/*
 * DoFunction.cpp
 *
 *  Created on: 17/01/2012
 *      Author: will
 */

#include "../mex_main.h"
#include "mex.h"

#include <iostream>
#include <fstream>

static const double eps = 2.22044604925031e-16;

#define VIS_DIV 100
#define NEW_MUL 50

CatGraph* deserializeGraph(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	string fileName(mxArrayToString(prhs[1]));
	mexPrintf("Using file %s\n", fileName.c_str());
	//ifstream ifs(fileName.c_str());
	//boost::archive::text_iarchive ia(ifs);

	catGraph = new CatGraph();
	mexPrintf("Instantiated CatGraph at %X\n", catGraph);
	//ifs >> catGraph;

	catGraph->deserializeGraph(fileName);
	return catGraph;
}

void serializeGraph(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	string fileName(mxArrayToString(prhs[1]));
	catGraph->serializeGraph(fileName);
}

CatGraph* initialiseGraph(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> numParticles(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> numNodes(mxGetPr(prhs[2]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> Q(mxGetPr(prhs[3]), 3, 3);
	const Eigen::Map<Eigen::MatrixXd> u_mean(mxGetPr(prhs[4]), 3, 1);

	catGraph = new CatGraph((unsigned int) numParticles(0, 0),
			(unsigned int) numNodes(0, 0), Q, u_mean);
	return catGraph;

}

void initialiseVision(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	string codebookPath(mxArrayToString(prhs[1]));
	string chowLiuPath(mxArrayToString(prhs[2]));
	catGraph->initialiseVision(codebookPath, chowLiuPath);
}

void initialiseTrajectory(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(prhs[1]), 1, 1);
	string descriptorPath(mxArrayToString(prhs[2]));

	cv::FileStorage descriptorFile(descriptorPath,cv::FileStorage::READ);
	cv::Mat* descriptors = new cv::Mat;

	descriptorFile["descriptors"] >> (*descriptors);
	descriptorFile.release();

	catGraph->initialiseTrajectory(descriptors, descriptorPath);
}

void initialiseParticles(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[1]), 1, 1);

	catGraph->initialiseParticles((unsigned int) blankFrames(0, 0));
}

void addNode(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> odometry(mxGetPr(prhs[2]), 3, 1);
	string descriptorPath(mxArrayToString(prhs[3]));

	cv::FileStorage descriptorFile(descriptorPath,cv::FileStorage::READ);
	cv::Mat* descriptors = new cv::Mat;

	descriptorFile["descriptors"] >> (*descriptors);
	descriptorFile.release();

	catGraph->addNode(descriptors, descriptorPath, odometry);
}

void updateInformation(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const int numFrames = mxGetM(prhs[1]);

	const Eigen::Map<Eigen::MatrixXd> fabmapPDF(mxGetPr(prhs[1]), numFrames,
			numFrames);
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[2]), 1, 1);

	unsigned int maxFrame = catGraph->frameNum - (unsigned int) blankFrames(0,
				0);

	catGraph->information.clear();
	GraphNode* node;
	for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
			!= catGraph->nodes.end(); nodeIter++) {
		node = *nodeIter;
		//mexPrintf("Node %X at %d ",node,node->frameNum);
		if (node->edgeFrom.size() && node->edgeTo.size()) {

			// build local map to depth of 1
			LocalGraph * localGraph = new LocalGraph(node, (unsigned int) 1);

			//mexPrintf("has %d edges:\n",localGraph->edges.size());
			//for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			//		!= localGraph->edges.end(); edgeIter++) {
			//	mexPrintf("  Edge %X from Node %X at %d to Node %X at %d\n",
			//			*edgeIter, (*edgeIter)->nodeFrom,
			//			(*edgeIter)->nodeFrom->frameNum, (*edgeIter)->nodeTo,
			//			(*edgeIter)->nodeTo->frameNum);
			//}

			// find all possible traverses of adjacent nodes

			//mexPrintf("Analysing possible traverses:\n");

			node->odoLikelihood = -DBL_MAX;
			node->visLikelihood = -DBL_MAX;

			GraphEdge *fromEdge, *toEdge;
			double odoFraction;
			double odoLikelihood;
			double visLikelihood;
			double information, maxInformation = -DBL_MAX;

			for (vector<GraphEdge*>::iterator fromIter = node->edgeFrom.begin(); fromIter
					!= node->edgeFrom.end(); fromIter++) {
				for (vector<GraphEdge*>::iterator toIter = node->edgeTo.begin(); toIter
						!= node->edgeTo.end(); toIter++) {
					fromEdge = *fromIter;
					toEdge = *toIter;



					Vector3d origin(0, 0, 0);
					Vector3d odometry = inverseModel(localGraph->nodes.find(
							toEdge->nodeTo)->second, localGraph->nodes.find(
							fromEdge->nodeFrom)->second);
					if (getRadius(odometry) < eps) {
						odometry(1) = odometry(1) + 1e-5;
					}

					odoFraction = get2DForwardDPx_2(localGraph->nodes.find(
							fromEdge->nodeFrom)->second,
							localGraph->nodes.find(toEdge->nodeTo)->second,
							origin, fromEdge->icov);
					if (odoFraction < 0)
						odoFraction = 0;
					if (odoFraction > 1)
						odoFraction = 1;
					odoLikelihood = get2DForwardLogPx(forwardModelFrac(
							localGraph->nodes.find(fromEdge->nodeFrom)->second,
							odometry, odoFraction), origin, fromEdge->icov);

					double fromEdgeVis, toEdgeVis;
					if (node->frameNum > fromEdge->nodeFrom->frameNum)
						fromEdgeVis = fabmapPDF(node->frameNum,
								fromEdge->nodeFrom->frameNum);
					else
						fromEdgeVis = fabmapPDF(fromEdge->nodeFrom->frameNum,
								node->frameNum);
					if (node->frameNum > toEdge->nodeTo->frameNum)
						toEdgeVis = fabmapPDF(node->frameNum,
								toEdge->nodeTo->frameNum);
					else
						toEdgeVis = fabmapPDF(toEdge->nodeTo->frameNum,
								node->frameNum);

					visLikelihood = (1 - odoFraction) * fromEdgeVis
							+ odoFraction * toEdgeVis;

					information = odoLikelihood + visLikelihood / VIS_DIV;

					//mexPrintf(
					//		"  Traversal from Node %X at %d to Node %X at %d:\n    Gives fraction %0.4f odo %0.4f vis %0.4f information %0.4f\n",
					//		fromEdge->nodeFrom, fromEdge->nodeFrom->frameNum,
					//		toEdge->nodeTo, toEdge->nodeTo->frameNum,
					//		odoFraction, odoLikelihood, visLikelihood,
					//		information);

					if (information > maxInformation) {
						node->odoLikelihood = odoLikelihood;
						node->visLikelihood = visLikelihood;
						node->bestParents = pair<GraphEdge*, GraphEdge*> (
								fromEdge, toEdge);
						maxInformation = information;
					}
				}
			}

			//mexPrintf(
			//	"Best traversal from Node %X at %d to Node %X at %d gives information %0.4f\n",
			//	node->bestParents.first->nodeFrom, node->bestParents.first->nodeFrom->frameNum,
			//	node->bestParents.second->nodeTo, node->bestParents.second->nodeTo->frameNum,
			//	maxInformation);

			if (maxInformation < -DBL_MAX / 2) {
				mexPrintf(
						"!!!! --- Max Information of node %X at %d is very low at %0.4f --- !!!!\n",
						node, node->frameNum, maxInformation);
			} else {

				catGraph->information.insert(pair<double, GraphNode*> (
						maxInformation, node));
			}
		} else {
			//mexPrintf("has insufficient edges, information = -DBL_MAX\n");

			//node->odoLikelihood = 0;
			//node->visLikelihood = -DBL_MAX;
			//node->bestParents = pair<GraphEdge*, GraphEdge*> (NULL, NULL);
			//
			//catGraph->information.insert(pair<double, GraphNode*> (-DBL_MAX,
			//		node));
		}

	}

}

void updateInformationOnline(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	catGraph->updateInformation();

}

void pruneNode(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[2]), 1, 1);

	if (catGraph->information.size() == 0) {
			mexPrintf("No suitable nodes to prune!\n");
		return;
	}


	unsigned int maxFrame = catGraph->frameNum - (unsigned int) blankFrames(0,
			0);

	double information = 0;
	GraphNode* node = NULL;
	if (frameNum(0, 0) == -1) {
		//node = catGraph->nodes.back();

		map<double, GraphNode*>::iterator randIndex =
				catGraph->information.begin();
		std::advance(randIndex, rand() % catGraph->information.size());
		node = randIndex->second;

		//mexPrintf("Randomly chose node %X at %d with parents %X and %X\n",
		//					node, node->frameNum, node->bestParents.first,
		//					node->bestParents.second);
		/*
		while (node->frameNum > maxFrame || node->bestParents.first == NULL
				|| node->bestParents.second == NULL
				|| node->bestParents.first->nodeFrom < (GraphNode*) 10000
				|| node->bestParents.second->nodeTo < (GraphNode*) 10000) {

			map<double, GraphNode*>::iterator randIndex =
					catGraph->information.begin();
			std::advance(randIndex, rand() % catGraph->information.size());
			node = randIndex->second;
			mexPrintf("Randomly chose node %X at %d with parents %X and %X\n",
					node, node->frameNum, node->bestParents.first,
					node->bestParents.second);
		}
		*/

		/*
		for (map<double, GraphNode*>::reverse_iterator mapIter =
				catGraph->information.rbegin(); mapIter
				!= catGraph->information.rend(); mapIter++) {
			if (mapIter->first < 0 && mapIter->second->frameNum < maxFrame
					&& mapIter->second->bestParents.first->nodeFrom->frameNum
							< maxFrame
					&& mapIter->second->bestParents.second->nodeTo->frameNum
							< maxFrame) {
				node = mapIter->second;
				information = mapIter->first;
				break;
			}
		}*/
	} else {
		for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
				!= catGraph->nodes.end(); nodeIter++) {
			if ((*nodeIter)->frameNum == (unsigned int) frameNum(0, 0)) {
				node = *nodeIter;
				break;
			}
		}
	}
	if (node == NULL)
		mexErrMsgTxt(
				"Frame number does not correspond to a node: already pruned?");

	//mexPrintf(
	//		"Parent edge %X from node %X to node %X, edge %X from node %X to node %X\n",
	//		node->bestParents.first, node->bestParents.first->nodeFrom, node->bestParents.first->nodeTo,
	//		node->bestParents.second, node->bestParents.second->nodeFrom, node->bestParents.second->nodeTo);

	//mexPrintf("Pruning node %X at %d information %0.4f to parents %X at %d and %X at %d:\n",
	//		node, node->frameNum, information, node->bestParents.first->nodeFrom,
	//		node->bestParents.first->nodeFrom->frameNum,
	//		node->bestParents.second->nodeTo,
	//		node->bestParents.second->nodeTo->frameNum);

	GraphNode* pNode;
	GraphEdge* pEdge;
	Particle* pParticle;

	plhs[10] = mxCreateDoubleMatrix(1, 1, mxREAL);
	Eigen::Map<Eigen::MatrixXd> p_frameNum(mxGetPr(plhs[10]), 1, 1);

	p_frameNum(0,0) = (double)node->frameNum;

	// build local map to depth of 1
	//mexPrintf("Building local map\n");
	LocalGraph * localGraph = new LocalGraph(node, (unsigned int) 1);

	// print nodes and edges
	/*
	mexPrintf("\n--- Local Graph of Node %X before Modification ---\n\n", node);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		pNode = nodeIter->first;
		if (pNode==node)
			mexPrintf("Deleting: ");
		mexPrintf("Node %X at frame %d has %d edges:\n", pNode,
				pNode->frameNum, pNode->edgeFrom.size() + pNode->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeFrom.begin(); edgeIter
				!= pNode->edgeFrom.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeFrom->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeTo.begin(); edgeIter
				!= pNode->edgeTo.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeTo->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
	}
	*/
	// output local graph
	//mexPrintf("Output local graph\n");
	unsigned int nnB = localGraph->nodes.size();
	unsigned int neB = 2 * localGraph->edges.size();

	plhs[0] = mxCreateDoubleMatrix(1, nnB, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(3, nnB, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, neB, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(3, neB, mxREAL);

	Eigen::Map<Eigen::MatrixXd> nodeIDBefore(mxGetPr(plhs[0]), 1, nnB);
	Eigen::Map<Eigen::MatrixXd> nodePosBefore(mxGetPr(plhs[1]), 3, nnB);
	Eigen::Map<Eigen::MatrixXd> edgeIDBefore(mxGetPr(plhs[2]), 1, neB);
	Eigen::Map<Eigen::MatrixXd> edgePosBefore(mxGetPr(plhs[3]), 3, neB);

	GraphNode *nodeB;
	GraphEdge *edgeB;
	unsigned int index = 0;

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		nodeB = nodeIter->first;
		nodeIDBefore(0, index) = nodeB->frameNum;
		nodePosBefore.block<3, 1> (0, index) = nodeIter->second;
		index++;
	}

	index = 0;
	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		edgeB = *edgeIter;
		edgeIDBefore(0, index) = edgeB->nodeFrom->frameNum;
		edgeIDBefore(0, index + 1) = edgeB->nodeTo->frameNum;
		edgePosBefore.block<3, 1> (0, index) = (localGraph->nodes.find(
				edgeB->nodeFrom))->second;
		edgePosBefore.block<3, 1> (0, index + 1) = (localGraph->nodes.find(
				edgeB->nodeTo))->second;
		index = index + 2;
	}

	// find and remove node and edges from global list
	//mexPrintf("Find and remove node and edges from global list\n");
	for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
			!= catGraph->nodes.end(); nodeIter++) {
		if (*nodeIter == node) {
			catGraph->nodes.erase(nodeIter);
			break;
		}
	}
	for (vector<GraphEdge*>::iterator edgeIter = catGraph->edges.begin(); edgeIter
			!= catGraph->edges.end(); edgeIter++) {
		if (localGraph->edges.find(*edgeIter) != localGraph->edges.end()) {
			catGraph->edges.erase(edgeIter);
			edgeIter--;
		}
	}

	// find and remove node from information content
	map<GraphNode*, multimap<double, GraphNode*>::iterator>::iterator
			currInformation = catGraph->informationLookup.find(node);
	if (currInformation != catGraph->informationLookup.end()) {
		mexPrintf(
				"Removing reference from node %X at %d to information %0.4f for node %X at %d\n",
				currInformation->first, currInformation->first->frameNum,
				currInformation->second->first,
				currInformation->second->second,
				currInformation->second->second->frameNum);
		catGraph->information.erase(currInformation->second);
		catGraph->informationLookup.erase(currInformation);
	} else {
		// BIG ERROR - Tried to remove node without valid information content!
		mexPrintf(
				"BIG ERROR - Tried to remove node without valid information content!\n");
	}

	// add neighbouring nodes to information content recalculation
	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		if (nodeIter->first != node)
			catGraph->updateList.push_back(nodeIter->first);
	}

	// find and remove edge references from linked nodes
	//mexPrintf("Find and remove edge references from linked nodes\n");
	//mexPrintf("Node %X:\n",node);

	for (vector<GraphEdge*>::iterator edgeFromIter = node->edgeFrom.begin(); edgeFromIter
			!= node->edgeFrom.end(); edgeFromIter++) {
		//mexPrintf("EdgeFrom %X from %X to %X\n",*edgeFromIter,(*edgeFromIter)->nodeFrom,(*edgeFromIter)->nodeTo);
		for (vector<GraphEdge*>::iterator edgeToIter =
				(*edgeFromIter)->nodeFrom->edgeTo.begin(); edgeToIter
				!= (*edgeFromIter)->nodeFrom->edgeTo.end(); edgeToIter++) {
			//mexPrintf("EdgeTo %X from %X to %X\n",*edgeToIter,(*edgeToIter)->nodeFrom,(*edgeToIter)->nodeTo);
			if (*edgeToIter==*edgeFromIter) {
				//mexPrintf("  Erasing edge...\n");
				(*edgeFromIter)->nodeFrom->edgeTo.erase(edgeToIter);
				break;
			}
		}
	}
	// find and remove edge references to linked nodes
	//mexPrintf("Find and remove edge references to linked nodes\n");
	for (vector<GraphEdge*>::iterator edgeToIter = node->edgeTo.begin(); edgeToIter
			!= node->edgeTo.end(); edgeToIter++) {
		//mexPrintf("EdgeTo %X from %X to %X\n",*edgeToIter,(*edgeToIter)->nodeFrom,(*edgeToIter)->nodeTo);
		for (vector<GraphEdge*>::iterator edgeFromIter =
				(*edgeToIter)->nodeTo->edgeFrom.begin(); edgeFromIter
				!= (*edgeToIter)->nodeTo->edgeFrom.end(); edgeFromIter++) {
			//mexPrintf("EdgeFrom %X from %X to %X\n",*edgeFromIter,(*edgeFromIter)->nodeFrom,(*edgeFromIter)->nodeTo);
			if (*edgeFromIter==*edgeToIter) {
				//mexPrintf("  Erasing edge...\n");
				(*edgeToIter)->nodeTo->edgeFrom.erase(edgeFromIter);
				break;
			}
		}
	}

	// find local particle positions
	//mexPrintf("Find local particle positions\n");
	map<Particle*, Vector3d> particleMap;
	Vector3d position;

	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		for (vector<Particle*>::iterator edgeParticleIter =
				(*edgeIter)->particles.begin(); edgeParticleIter
				!= (*edgeIter)->particles.end(); edgeParticleIter++) {
			position = forwardModelFrac(localGraph->nodes.find(
					(*edgeIter)->nodeFrom)->second, (*edgeIter)->odometry,
					(*edgeParticleIter)->fraction);
			if ((*edgeParticleIter)->direction)
				position(2) = constrainAngle(position(2) + pi);

			//mexPrintf(
			//		"Particle %X on edge %X (%X) between %d and %d at fraction %0.4f gives local position %0.4f %0.4f %0.4f\n",
			//		*edgeParticleIter, *edgeIter, (*edgeParticleIter)->root,
			//		(*edgeIter)->nodeFrom->frameNum,
			//		(*edgeIter)->nodeTo->frameNum,
			//		(*edgeParticleIter)->fraction, position(0), position(1),
			//		position(2));

			particleMap.insert(pair<Particle*, Vector3d> (*edgeParticleIter,
					position));
		}
	}
	/*
	mexPrintf("Reading out particle map:\n");
	for (map<Particle*, Vector3d>::iterator particleIter = particleMap.begin(); particleIter
			!= particleMap.end(); particleIter++) {
		mexPrintf(
				"Particle %X on edge %X between %d and %d at fraction %0.4f gives local position %0.4f %0.4f %0.4f\n",
				particleIter->first, particleIter->first->root, particleIter->first->root->nodeFrom->frameNum,
				particleIter->first->root->nodeTo->frameNum,
				particleIter->first->fraction, particleIter->second(0), particleIter->second(1),
				particleIter->second(2));
		index++;
	}
	*/

	// output local particles
	//mexPrintf("Output local particles\n");

	unsigned int np = particleMap.size();
	plhs[4] = mxCreateDoubleMatrix(3, np, mxREAL);
	Eigen::Map<Eigen::MatrixXd> particlePosBefore(mxGetPr(plhs[4]), 3, np);

	index = 0;
	for (map<Particle*, Vector3d>::iterator particleIter = particleMap.begin(); particleIter
			!= particleMap.end(); particleIter++) {
		particlePosBefore.block<3, 1> (0, index) = particleIter->second;
		index++;
	}

	// print nodes and edges
	/*
	mexPrintf("\n--- Local Graph of Node %X after Edge Removal ---\n\n", node);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		pNode = nodeIter->first;
		if (pNode==node)
			mexPrintf("Deleting: ");
		mexPrintf("Node %X at frame %d has %d edges:\n", pNode,
				pNode->frameNum, pNode->edgeFrom.size() + pNode->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeFrom.begin(); edgeIter
				!= pNode->edgeFrom.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeFrom->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeTo.begin(); edgeIter
				!= pNode->edgeTo.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeTo->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
	}
	*/
	// create equivalent main edge
	//mexPrintf("Create equivalent main edge\n");

	vector<GraphEdge*> newEdges;

	Vector3d odometry = inverseModel(localGraph->nodes.find(
			node->bestParents.second->nodeTo)->second, localGraph->nodes.find(
			node->bestParents.first->nodeFrom)->second);
	Matrix3d icov = getInverseCovariance(odometry, catGraph->Q);
	if (getRadius(odometry) < eps) {
		odometry(1) = odometry(1) + 1e-5;
	}

	// big bug - make sure the information is updated before calling prune twice

	GraphEdge* edge = node->bestParents.first->nodeFrom->addEdgeTo(
			node->bestParents.second->nodeTo, odometry, icov);
	//mexPrintf("New edge %X from node %X at %d to node %X at %d!\n", edge,
	//		edge->nodeFrom, edge->nodeFrom->frameNum, edge->nodeTo,
	//		edge->nodeTo->frameNum);
	catGraph->edges.push_back(edge);
	newEdges.push_back(edge);

	// print nodes and edges
	/*
	mexPrintf("\n--- Local Graph of Node %X after Edges between Parents ---\n\n",node);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		pNode = nodeIter->first;
		if (pNode == node)
			mexPrintf("Deleting: ");
		mexPrintf("Node %X at frame %d has %d edges:\n", pNode, pNode->frameNum,
				pNode->edgeFrom.size() + pNode->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeFrom.begin(); edgeIter
				!= pNode->edgeFrom.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeFrom->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0), pEdge->odometry(
							2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeTo.begin(); edgeIter
				!= pNode->edgeTo.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeTo->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0), pEdge->odometry(
							2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
	}
	*/
	// add links between from nodes
	//mexPrintf("Add links between from nodes\n");
	for (vector<GraphEdge*>::iterator edgeFromIter = node->edgeFrom.begin(); edgeFromIter
			!= node->edgeFrom.end(); edgeFromIter++) {
		if ((*edgeFromIter)->nodeFrom != node->bestParents.first->nodeFrom) {
			odometry = inverseModel(localGraph->nodes.find(
					node->bestParents.first->nodeFrom)->second,
					localGraph->nodes.find((*edgeFromIter)->nodeFrom)->second);
			icov = getInverseCovariance(odometry, catGraph->Q);
			if (getRadius(odometry)<eps) {
				odometry(1) = odometry(1) + 1e-5;
			}

			edge = (*edgeFromIter)->nodeFrom->addEdgeTo(
					node->bestParents.first->nodeFrom, odometry, icov);
			//mexPrintf("New edge %X from node %X at %d to node %X at %d!\n", edge,
			//			edge->nodeFrom, edge->nodeFrom->frameNum, edge->nodeTo,
			//			edge->nodeTo->frameNum);
			catGraph->edges.push_back(edge);
			newEdges.push_back(edge);
		}
	}
	// add links between to nodes
	//mexPrintf("Add links between to nodes\n");
	for (vector<GraphEdge*>::iterator edgeToIter = node->edgeTo.begin(); edgeToIter
			!= node->edgeTo.end(); edgeToIter++) {
		if ((*edgeToIter)->nodeTo != node->bestParents.second->nodeTo) {
			odometry = inverseModel(localGraph->nodes.find(
					node->bestParents.second->nodeTo)->second,
					localGraph->nodes.find((*edgeToIter)->nodeTo)->second);
			icov = getInverseCovariance(odometry, catGraph->Q);
			if (getRadius(odometry) < eps) {
				odometry(1) = odometry(1) + 1e-5;
			}

			edge = (*edgeToIter)->nodeTo->addEdgeTo(
					node->bestParents.second->nodeTo, odometry, icov);
			//mexPrintf("New edge %X from node %X at %d to node %X at %d!\n", edge,
			//			edge->nodeFrom, edge->nodeFrom->frameNum, edge->nodeTo,
			//			edge->nodeTo->frameNum);
			catGraph->edges.push_back(edge);
			newEdges.push_back(edge);
		}
	}

	/*
	// print nodes and edges

	mexPrintf("\n--- Local Graph of Node %X after Remaining Edges ---\n\n", node);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		pNode = nodeIter->first;
		if (pNode == node)
			mexPrintf("Deleting: ");
		mexPrintf("Node %X at frame %d has %d edges:\n", pNode,
				pNode->frameNum, pNode->edgeFrom.size() + pNode->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeFrom.begin(); edgeIter
				!= pNode->edgeFrom.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeFrom->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeTo.begin(); edgeIter
				!= pNode->edgeTo.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeTo->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0),
					pEdge->odometry(2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
	}*/

	// output new local graph
	//mexPrintf("Output new local graph\n");

	unsigned int nnA = localGraph->nodes.size() - 1;
	unsigned int neA = 2 * newEdges.size();

	plhs[5] = mxCreateDoubleMatrix(1, nnA, mxREAL);
	plhs[6] = mxCreateDoubleMatrix(3, nnA, mxREAL);
	plhs[7] = mxCreateDoubleMatrix(1, neA, mxREAL);
	plhs[8] = mxCreateDoubleMatrix(3, neA, mxREAL);

	Eigen::Map<Eigen::MatrixXd> nodeIDAfter(mxGetPr(plhs[5]), 1, nnA);
	Eigen::Map<Eigen::MatrixXd> nodePosAfter(mxGetPr(plhs[6]), 3, nnA);
	Eigen::Map<Eigen::MatrixXd> edgeIDAfter(mxGetPr(plhs[7]), 1, neA);
	Eigen::Map<Eigen::MatrixXd> edgePosAfter(mxGetPr(plhs[8]), 3, neA);

	index = 0;
	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		nodeB = nodeIter->first;
		if (nodeB != node) {
			nodeIDAfter(0, index) = nodeB->frameNum;
			nodePosAfter.block<3, 1> (0, index) = nodeIter->second;
			index++;
		}
	}

	index = 0;
	for (vector<GraphEdge*>::iterator edgeIter = newEdges.begin(); edgeIter
			!= newEdges.end(); edgeIter++) {
		edgeB = *edgeIter;
		edgeIDAfter(0, index) = edgeB->nodeFrom->frameNum;
		edgeIDAfter(0, index + 1) = edgeB->nodeTo->frameNum;
		edgePosAfter.block<3, 1> (0, index) = (localGraph->nodes.find(
				edgeB->nodeFrom))->second;
		edgePosAfter.block<3, 1> (0, index + 1) = (localGraph->nodes.find(
				edgeB->nodeTo))->second;
		index = index + 2;
	}

	// update affected particles
	//mexPrintf("Update affected particles\n");
	Particle* particle;
	Vector3d origin(0, 0, 0);

	for (map<Particle*, Vector3d>::iterator particleIter = particleMap.begin(); particleIter
			!= particleMap.end(); particleIter++) {
		particle = particleIter->first;
		double likelihood, maxLikelihood = -DBL_MAX;
		double alpha, maxAlpha = 0;
		GraphEdge* maxEdge = newEdges.front();

		for (vector<GraphEdge*>::iterator edgeIter = newEdges.begin(); edgeIter
				!= newEdges.end(); edgeIter++) {
			edge = *edgeIter;
			alpha = get2DForwardDPx_2(
					localGraph->nodes.find(edge->nodeFrom)->second,
					localGraph->nodes.find(edge->nodeTo)->second, particleIter->second,
					edge->icov);
			if (alpha < 0)
				alpha = 0;
			if (alpha > 1)
				alpha = 1;
			likelihood = get2DForwardLogPx(forwardModelFrac(
					localGraph->nodes.find(edge->nodeFrom)->second,
					edge->odometry, alpha), particleIter->second, edge->icov);
			if (likelihood > maxLikelihood) {
				maxLikelihood = likelihood;
				maxAlpha = alpha;
				maxEdge = edge;
			}

		}

		Vector3d maxEdgePos = forwardModelFrac(localGraph->nodes.find(
				maxEdge->nodeFrom)->second, maxEdge->odometry, maxAlpha);

		double forwardAngDiff = angleDistance(maxEdgePos(2), particleIter->second(2));
		double reverseAngDiff = angleDistance(
				constrainAngle(maxEdgePos(2) + pi), particleIter->second(2));
		bool maxDirection = abs(forwardAngDiff) > abs(reverseAngDiff);

		particle->root = maxEdge;
		particle->fraction = maxAlpha;
		particle->odoLikelihood = maxLikelihood;
		particle->direction = maxDirection;
		particle->root->particles.push_back(particle);

	}

	// print nodes and edges
	/*
	mexPrintf("\n--- Local Graph of Node %X after Particle Update ---\n\n",node);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		pNode = nodeIter->first;
		if (pNode == node)
			mexPrintf("Deleting: ");
		mexPrintf("Node %X at frame %d has %d edges:\n", pNode,
				pNode->frameNum, pNode->edgeFrom.size() + pNode->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeFrom.begin(); edgeIter
				!= pNode->edgeFrom.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeFrom->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0), pEdge->odometry(
							2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = pNode->edgeTo.begin(); edgeIter
				!= pNode->edgeTo.end(); edgeIter++) {
			pEdge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					pEdge, pEdge->nodeTo->frameNum, pEdge->frameNum,
					pEdge->odometry(0, 0), pEdge->odometry(1, 0), pEdge->odometry(
							2, 0));
			for (vector<Particle*>::iterator particleIter =
					pEdge->particles.begin(); particleIter
					!= pEdge->particles.end(); particleIter++) {
				pParticle = *particleIter;
				mexPrintf(
						"    Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d\n",
						pParticle, pParticle->root,
						pParticle->root->nodeFrom->frameNum,
						pParticle->root->nodeTo->frameNum, pParticle->fraction,
						pParticle->direction);
			}
		}
	}
	*/
	// output updated particles
	//mexPrintf("Output updated particles\n");

	plhs[9] = mxCreateDoubleMatrix(3, np, mxREAL);
	Eigen::Map<Eigen::MatrixXd> particlePosAfter(mxGetPr(plhs[9]), 3, np);

	index = 0;
	for (map<Particle*, Vector3d>::iterator particleIter = particleMap.begin(); particleIter
			!= particleMap.end(); particleIter++) {
		Vector3d particlePos = forwardModelFrac(localGraph->nodes.find(
				particleIter->first->root->nodeFrom)->second,
				particleIter->first->root->odometry,
				particleIter->first->fraction);
		if (particleIter->first->direction)
			particlePos(2) = constrainAngle(particlePos(2) + pi);
		particlePosAfter.block<3, 1> (0, index) = particlePos;
		index++;
	}

	// delete node and edges
	//mexPrintf("Delete node %X and edges\n",node);

	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		//mexPrintf("Deleting edge %X between %X and %X\n",*edgeIter,(*edgeIter)->nodeFrom,(*edgeIter)->nodeTo);
		delete *edgeIter;
	}

	//mexPrintf("Deleting local graph\n");
	delete localGraph;

	//mexPrintf("Deleting node %X\n",node);
	delete node;

	//mexPrintf("Complete!\n");

}

void pruneNodeOnline(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[1]), 1, 1);

	map<double, GraphNode*>::reverse_iterator mapIter =
					catGraph->information.rbegin();


	// min information method
	/*
	while (mapIter != catGraph->information.rend() && mapIter->second->frameNum
			> catGraph->nodes.back()->frameNum - (unsigned int) blankFrames(0,
					0) && mapIter->second->bestParents.first->nodeFrom
			!= mapIter->second->bestParents.second->nodeTo) {
		mapIter++;
	}

	catGraph->pruneNode(mapIter->second);
*/

	// stochastic method

	double subset = 2;

	std::advance(mapIter, rand() % (unsigned int) catGraph->information.size()
			/ subset);
	while (mapIter != catGraph->information.rend() && mapIter->second->frameNum
			> catGraph->nodes.back()->frameNum - (unsigned int) blankFrames(0,
					0) && mapIter->second->bestParents.first->nodeFrom
			!= mapIter->second->bestParents.second->nodeTo) {
		mapIter++;
	}

	catGraph->pruneNode(mapIter->second);



	plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
	Eigen::Map<Eigen::MatrixXd> p_frameNum(mxGetPr(plhs[0]), 1, 1);

	p_frameNum(0,0) = (double)mapIter->second->frameNum;


}


void updateParticlePositions(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> odometry(mxGetPr(prhs[1]), 3, 1);
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[2]), 1, 1);

	catGraph->updateParticlePositions(odometry,
			(unsigned int) blankFrames(0, 0));
}

void updateParticleWeightsOnline(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	catGraph->updateParticleWeights();
}

void updateParticleWeights(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	//catGraph->updateParticleWeights(); // integrate OpenFAB-MAP when required

	const int numFrames = mxGetM(prhs[1]);

	const Eigen::Map<Eigen::MatrixXd> fabmapPDF(mxGetPr(prhs[1]), numFrames,
			numFrames);
	const Eigen::Map<Eigen::MatrixXd> fabmapAvg(mxGetPr(prhs[2]), numFrames, 1);

	Particle *particle;
	double weightSum = 0;
	for (vector<Particle*>::iterator particleIter = catGraph->particles.begin(); particleIter
			!= catGraph->particles.end(); particleIter++) {
		particle = *particleIter;

		particle->visLikelihood = (1 - particle->fraction) * fabmapPDF(
				catGraph->frameNum, particle->root->nodeFrom->frameNum)
				+ particle->fraction * fabmapPDF(catGraph->frameNum,
						particle->root->nodeTo->frameNum);
		particle->weight = particle->weight
				* exp(0.5 * (particle->odoLikelihood + particle->visLikelihood
						/ VIS_DIV));
		weightSum += particle->weight;
	}
	catGraph->visLikelihood_mean = fabmapAvg(catGraph->frameNum, 0);
	catGraph->w_mean = NEW_MUL / (double) catGraph->numParticles * exp(0.5
			* (catGraph->odoLikelihood_mean + catGraph->visLikelihood_mean
					/ VIS_DIV));
	weightSum += catGraph->w_mean;

	for (vector<Particle*>::iterator particleIter = catGraph->particles.begin(); particleIter
			!= catGraph->particles.end(); particleIter++) {
		particle = *particleIter;
		particle->weight /= weightSum;
		if (particle->weight < eps)
			particle->weight = eps;
	}
	catGraph->w_mean /= weightSum;

	mexPrintf("New Place likelihood: %%%0.4f\n", catGraph->w_mean * 100);

}

void calculateESS(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
	Eigen::Map<Eigen::MatrixXd> ESS(mxGetPr(plhs[0]), 1, 1);

	ESS(0, 0) = catGraph->calculateESS();
}

void calculateHypothesis(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> hypothesisDist(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[1]), 1, 1);

	catGraph->calculateHypothesis(hypothesisDist(0, 0),
			(unsigned int) blankFrames(0, 0));

	plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(2, 1, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL);
	Eigen::Map<Eigen::MatrixXd> index(mxGetPr(plhs[0]), 1, 1);
	Eigen::Map<Eigen::MatrixXd> direction(mxGetPr(plhs[1]), 1, 1);
	Eigen::Map<Eigen::MatrixXd> hypothesis(mxGetPr(plhs[2]), 1, 1);
	Eigen::Map<Eigen::MatrixXd> edgeID(mxGetPr(plhs[3]), 2, 1);
	Eigen::Map<Eigen::MatrixXd> fraction(mxGetPr(plhs[4]), 1, 1);

	index(0, 0) = 0;
	direction(0, 0) = 0;
	hypothesis(0, 0) = 0;
	Particle *particle;
	unsigned int particleIndex = 0;
	for (vector<Particle*>::iterator particleIter = catGraph->particles.begin(); particleIter
			!= catGraph->particles.end(); particleIter++) {
		particle = *particleIter;
		if (particle->hypothesis > hypothesis(0, 0)) {
			hypothesis(0, 0) = particle->hypothesis;
			index(0, 0) = particleIndex;
			direction(0, 0) = particle->direction;
			edgeID(0, 0) = particle->root->nodeFrom->frameNum;
			edgeID(1, 0) = particle->root->nodeTo->frameNum;
			fraction(0, 0) = particle->fraction;
		}
		particleIndex++;
	}
}

void addLoopClosure(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> particleNo(mxGetPr(prhs[1]), 1, 1);

	if (particleNo(0, 0) > catGraph->particles.size()) {
		mexErrMsgTxt("Particle index greater than numParticles");
	}

	catGraph->addLoopClosure(catGraph->particles.at((int) particleNo(0, 0)));
}

void resampleParticles(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> spreadFraction(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> reverseFraction(mxGetPr(prhs[2]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> blankFrames(mxGetPr(prhs[3]), 1, 1);

	catGraph->resampleParticles(spreadFraction(0, 0), reverseFraction(0, 0),
			(unsigned int) blankFrames(0, 0));
}

void buildCodebookFromMap(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> numWords(mxGetPr(prhs[1]), 1, 1);
	string codebookPath(mxArrayToString(prhs[2]));
	catGraph->buildCodebookFromMap((unsigned int) numWords(0, 0), codebookPath);
}

void buildChowLiuFromMap(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	const Eigen::Map<Eigen::MatrixXd> infoThreshold(mxGetPr(prhs[1]), 1, 1);
	string chowLiuPath(mxArrayToString(prhs[2]));
	catGraph->buildChowLiuFromMap(infoThreshold(0, 0), chowLiuPath);
}

void recomputeWords(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	catGraph->recomputeWords();
}

void recomputeInformation(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {
	catGraph->recomputeInformation();
}

void printNodes(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	mexPrintf("Node information:\n");
	mexPrintf("Number of nodes: %d\n", catGraph->nodes.size());
	mexPrintf("Number of edges: %d\n", catGraph->edges.size());
	GraphNode *node;
	GraphEdge *edge;
	for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
			!= catGraph->nodes.end(); nodeIter++) {
		node = *nodeIter;
		mexPrintf(
				"Node %X at frame %d has path %s and %d edges:\n",
				node, node->frameNum, node->descriptorPath.c_str(), node->edgeFrom.size()
						+ node->edgeTo.size());
		for (vector<GraphEdge*>::iterator edgeIter = node->edgeFrom.begin(); edgeIter
				!= node->edgeFrom.end(); edgeIter++) {
			edge = *edgeIter;
			mexPrintf(
					"   Edge %X from node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					edge, edge->nodeFrom->frameNum, edge->frameNum,
					edge->odometry(0, 0), edge->odometry(1, 0), edge->odometry(
							2, 0));
		}
		for (vector<GraphEdge*>::iterator edgeIter = node->edgeTo.begin(); edgeIter
				!= node->edgeTo.end(); edgeIter++) {
			edge = *edgeIter;
			mexPrintf(
					"   Edge %X to node %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					edge, edge->nodeTo->frameNum, edge->frameNum,
					edge->odometry(0, 0), edge->odometry(1, 0), edge->odometry(
							2, 0));
		}

	}

}

void printEdges(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	mexPrintf("Edge information:\n");
	mexPrintf("Number of edges: %d\n", catGraph->edges.size());
	GraphEdge *edge;
	Particle *particle;
	unsigned int numParticles = 0;

	set<GraphEdge*> edgeSet;

	for (vector<GraphEdge*>::iterator edgeIter = catGraph->edges.begin(); edgeIter
			!= catGraph->edges.end(); edgeIter++) {
		edge = *edgeIter;
		edgeSet.insert(edge);
		numParticles += edge->particles.size();
		mexPrintf(
				"Edge %X between nodes %d and %d created at %d, odometry %0.4f %0.4f %0.4f has %d particles\n",
				edge, edge->nodeFrom->frameNum, edge->nodeTo->frameNum,
				edge->frameNum, edge->odometry(0, 0), edge->odometry(1, 0),
				edge->odometry(2, 0), edge->particles.size());
		for (vector<Particle*>::iterator particleIter = edge->particles.begin(); particleIter
				!= edge->particles.end(); particleIter++) {
			particle = *particleIter;
			mexPrintf(
					"  Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d has P_z %0.4f P_x %0.4f weight %0.4f hypothesis %0.4f\n",
					particle, particle->root,
					particle->root->nodeFrom->frameNum,
					particle->root->nodeTo->frameNum, particle->fraction,
					particle->direction, particle->visLikelihood,
					particle->odoLikelihood, particle->weight,
					particle->hypothesis);
		}
	}
	mexPrintf("Listed edges: %d Unique edges: %d Total particles: %d\n",
			catGraph->edges.size(), edgeSet.size(), numParticles);
}

void printParticles(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	mexPrintf("Particle information:\n");
	mexPrintf("Number of particles: %d\n", catGraph->particles.size());
	Particle *particle;
	for (vector<Particle*>::iterator particleIter = catGraph->particles.begin(); particleIter
			!= catGraph->particles.end(); particleIter++) {
		particle = *particleIter;
		mexPrintf(
				"Particle %X on edge %X between nodes %d and %d at fraction %0.4f direction %d has P_z %0.4f P_x %0.4f weight %0.4f hypothesis %0.4f\n",
				particle, particle->root, particle->root->nodeFrom->frameNum,
				particle->root->nodeTo->frameNum, particle->fraction,
				particle->direction, particle->visLikelihood,
				particle->odoLikelihood, particle->weight, particle->hypothesis);
	}
}

void printLocalGraph(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	mexPrintf("Local graph information:\n");
	GraphNode *node;
	GraphEdge *edge;
	Particle *particle;
	LocalGraph *localGraph;
	for (vector<Particle*>::iterator particleIter = catGraph->particles.begin(); particleIter
			!= catGraph->particles.end(); particleIter++) {
		particle = *particleIter;
		mexPrintf(
				"Particle %X on edge %X between nodes %d and %d at fraction %0.4f:\n",
				particle, particle->root, particle->root->nodeFrom->frameNum,
				particle->root->nodeTo->frameNum, particle->fraction);
		localGraph = new LocalGraph(particle, 2.0, catGraph->frameNum);
		mexPrintf("Local graph has %d nodes and %d edges:\n",
				localGraph->nodes.size(), localGraph->edges.size());
		for (map<GraphNode*, Vector3d>::iterator nodeIter =
				localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
			node = nodeIter->first;
			mexPrintf(
					"   Node %X at frame %d has %d edges, relative position %0.4f %0.4f %0.4f\n",
					node, node->frameNum, node->edgeFrom.size()
							+ node->edgeTo.size(), nodeIter->second(0, 0),
					nodeIter->second(1, 0), nodeIter->second(2, 0));
		}
		for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
				!= localGraph->edges.end(); edgeIter++) {
			edge = *edgeIter;
			mexPrintf(
					"   Edge %X between nodes %d and %d created at %d, odometry %0.4f %0.4f %0.4f\n",
					edge, edge->nodeFrom->frameNum, edge->nodeTo->frameNum,
					edge->frameNum, edge->odometry(0, 0), edge->odometry(1, 0),
					edge->odometry(2, 0));
		}

		delete localGraph;
	}

}

void printInformation(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {
	mexPrintf("Information content:\n");
	for (map<double, GraphNode*>::iterator nodeIter =
			catGraph->information.begin(); nodeIter
			!= catGraph->information.end(); nodeIter++) {
		mexPrintf("Node %X at %d has information content %0.4f\n",
				nodeIter->second, nodeIter->second->frameNum, nodeIter->first);
	}

}

void outputLocalGraph(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	const Eigen::Map<Eigen::MatrixXd> particleNo(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> radius(mxGetPr(prhs[2]), 1, 1);

	if (particleNo(0, 0) > catGraph->particles.size()) {
		mexErrMsgTxt("Particle index greater than numParticles");
	}

	LocalGraph *localGraph = new LocalGraph(catGraph->particles.at(
			(int) particleNo(0, 0)), radius(0, 0), catGraph->frameNum);

	unsigned int n = localGraph->nodes.size();
	unsigned int m = 2 * localGraph->edges.size();

	plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(3, n, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, m, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(3, m, mxREAL);

	Eigen::Map<Eigen::MatrixXd> nodeID(mxGetPr(plhs[0]), 1, n);
	Eigen::Map<Eigen::MatrixXd> nodePos(mxGetPr(plhs[1]), 3, n);
	Eigen::Map<Eigen::MatrixXd> edgeID(mxGetPr(plhs[2]), 1, m);
	Eigen::Map<Eigen::MatrixXd> edgePos(mxGetPr(plhs[3]), 3, m);

	GraphNode *node;
	GraphEdge *edge;
	unsigned int index = 0;

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		node = nodeIter->first;
		nodeID(0, index) = node->frameNum;
		nodePos.block<3, 1> (0, index) = nodeIter->second;
		index++;
	}

	index = 0;
	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		edge = *edgeIter;
		edgeID(0, index) = edge->nodeFrom->frameNum;
		edgeID(0, index + 1) = edge->nodeTo->frameNum;
		edgePos.block<3, 1> (0, index)
				= (localGraph->nodes.find(edge->nodeFrom))->second;
		edgePos.block<3, 1> (0, index + 1) = (localGraph->nodes.find(
				edge->nodeTo))->second;
		index = index + 2;
	}

	delete localGraph;

}

void outputLocalUpdate(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	const Eigen::Map<Eigen::MatrixXd> odometry(mxGetPr(prhs[1]), 3, 1);
	const Eigen::Map<Eigen::MatrixXd> particleNo(mxGetPr(prhs[2]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> maxFrame(mxGetPr(prhs[3]), 1, 1);

	if (particleNo(0, 0) > catGraph->particles.size()) {
		mexErrMsgTxt("Particle index greater than numParticles");
	}

	plhs[7] = mxCreateDoubleMatrix(1, 1, mxREAL);
	plhs[8] = mxCreateDoubleMatrix(3, 3, mxREAL);
	plhs[9] = mxCreateDoubleMatrix(3, 1, mxREAL);
	plhs[10] = mxCreateDoubleMatrix(1, 3, mxREAL);
	plhs[11] = mxCreateDoubleMatrix(3, 1, mxREAL);
	plhs[12] = mxCreateDoubleMatrix(1, 1, mxREAL);

	Eigen::Map<Eigen::MatrixXd> radius(mxGetPr(plhs[7]), 1, 1);
	Eigen::Map<Eigen::MatrixXd> icov(mxGetPr(plhs[8]), 3, 3);
	Eigen::Map<Eigen::MatrixXd> position(mxGetPr(plhs[9]), 3, 1);
	Eigen::Map<Eigen::MatrixXd> maxEdgeID(mxGetPr(plhs[10]), 1, 3);
	Eigen::Map<Eigen::MatrixXd> maxEdgePos(mxGetPr(plhs[11]), 3, 1);
	Eigen::Map<Eigen::MatrixXd> maxEdgeP(mxGetPr(plhs[12]), 1, 1);

	Particle *particle = catGraph->particles.at((int) particleNo(0, 0));

	GraphNode *node;
	GraphEdge *edge;
	unsigned int index = 0;

	// add odometry noise
	Vector3d origin(0, 0, 0);
	Vector3d normal = Vector3d(randn(0, 1), randn(0, 1), randn(0, 1));
	Vector3d odometry_n = odometry + catGraph->Q * normal;
	position = forwardModel(origin, odometry_n);

	icov = getInverseCovariance(odometry_n, catGraph->Q);

	//mexPrintf("Odometry %0.4f %0.4f %0.4f\n",odometry(0),odometry(1),odometry(2));
	//mexPrintf("Normal %0.4f %0.4f %0.4f\n",normal(0),normal(1),normal(2));
	//mexPrintf("Odometry_n %0.4f %0.4f %0.4f\n",odometry_n(0),odometry_n(1),odometry_n(2));
	//mexPrintf("Position %0.4f %0.4f %0.4f\n",position(0),position(1),position(2));

	// build local graph
	radius(0, 0) = 2 * getRadius(odometry_n) + 1.0;
	LocalGraph* localGraph = new LocalGraph(particle, radius(0, 0),
			(unsigned int) maxFrame(0, 0));

	unsigned int n = localGraph->nodes.size();
	unsigned int m = 2 * localGraph->edges.size();

	plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(3, n, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(2, n, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(1, m, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(3, m, mxREAL);
	plhs[5] = mxCreateDoubleMatrix(1, m, mxREAL);
	plhs[6] = mxCreateDoubleMatrix(1, m, mxREAL);

	Eigen::Map<Eigen::MatrixXd> nodeID(mxGetPr(plhs[0]), 1, n);
	Eigen::Map<Eigen::MatrixXd> nodePos(mxGetPr(plhs[1]), 3, n);
	Eigen::Map<Eigen::MatrixXd> nodeP(mxGetPr(plhs[2]), 2, n);
	Eigen::Map<Eigen::MatrixXd> edgeID(mxGetPr(plhs[3]), 1, m);
	Eigen::Map<Eigen::MatrixXd> edgePos(mxGetPr(plhs[4]), 3, m);
	Eigen::Map<Eigen::MatrixXd> edgeP(mxGetPr(plhs[5]), 1, m);
	Eigen::Map<Eigen::MatrixXd> edgeAlpha(mxGetPr(plhs[6]), 1, m);

	for (map<GraphNode*, Vector3d>::iterator nodeIter =
			localGraph->nodes.begin(); nodeIter != localGraph->nodes.end(); nodeIter++) {
		node = nodeIter->first;
		//mexPrintf("Output node %X\n",node);
		nodeID(0, index) = node->frameNum;
		nodePos.block<3, 1> (0, index) = nodeIter->second;
		nodeP(0, index) = get2DForwardLogPx(nodeIter->second, position, icov);
		nodeP(1, index) = get2DReverseLogPx(nodeIter->second, position, icov);
		index++;
	}

	double likelihood, maxLikelihood = -DBL_MAX;
	double alpha, maxAlpha = 0;
	GraphEdge* maxEdge = particle->root;
	bool maxDirection;

	index = 0;
	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		edge = *edgeIter;

		edgeID(0, index) = edge->nodeFrom->frameNum;
		edgeID(0, index + 1) = edge->nodeTo->frameNum;
		edgePos.block<3, 1> (0, index)
				= (localGraph->nodes.find(edge->nodeFrom))->second;
		edgePos.block<3, 1> (0, index + 1) = (localGraph->nodes.find(
				edge->nodeTo))->second;

		// forward
		alpha = get2DForwardDPx_2(
				localGraph->nodes.find(edge->nodeFrom)->second,
				localGraph->nodes.find(edge->nodeTo)->second, position, icov);
		//mexPrintf("Forward Edge between %d and %d has alpha %0.4f, ",
		//		edge->nodeFrom->frameNum, edge->nodeTo->frameNum, alpha);
		if (alpha < 0)
			alpha = 0;
		if (alpha > 1)
			alpha = 1;
		likelihood
				= get2DForwardLogPx(forwardModelFrac(localGraph->nodes.find(
						edge->nodeFrom)->second, edge->odometry, alpha),
						position, icov);
		//mexPrintf("bound to %0.4f, yields likelihood %0.4f\n", alpha,
		//		likelihood);
		if (likelihood > maxLikelihood) {
			maxLikelihood = likelihood;
			maxAlpha = alpha;
			maxEdge = edge;
			maxDirection = false; // forwards
		}

		edgeAlpha(0, index) = alpha;
		edgeP(0, index) = likelihood;

		// reverse
		/*
		 alpha = get2DReverseDPx_2(localGraph->nodes.find(edge->nodeFrom)->second,
		 localGraph->nodes.find(edge->nodeTo)->second, position, icov);
		 mexPrintf("Reverse Edge between %d and %d has alpha %0.4f, ",
		 edge->nodeFrom->frameNum, edge->nodeTo->frameNum, alpha);
		 if (alpha < 0)
		 alpha = 0;
		 if (alpha > 1)
		 alpha = 1;
		 likelihood
		 = get2DReverseLogPx(forwardModelFrac(localGraph->nodes.find(
		 edge->nodeFrom)->second, edge->odometry, alpha),
		 position, icov);
		 mexPrintf("bound to %0.4f, yields likelihood %0.4f\n", alpha,
		 likelihood);
		 if (likelihood > maxLikelihood) {
		 maxLikelihood = likelihood;
		 maxAlpha = alpha;
		 maxEdge = edge;
		 maxDirection = true; // reverse
		 }
		 */
		edgeAlpha(0, index + 1) = alpha;
		edgeP(0, index + 1) = likelihood;

		index = index + 2;
	}

	maxEdgeID(0, 0) = maxEdge->nodeFrom->frameNum;
	maxEdgeID(0, 1) = maxEdge->nodeTo->frameNum;
	maxEdgeID(0, 2) = (int) maxDirection;

	maxEdgePos = forwardModelFrac(
			localGraph->nodes.find(maxEdge->nodeFrom)->second,
			maxEdge->odometry, maxAlpha);

	double forwardAngDiff = angleDistance(maxEdgePos(2), position(2));
	double reverseAngDiff = angleDistance(constrainAngle(maxEdgePos(2) + pi),
			position(2));
	//mexPrintf("Forward angle diff %0.4f Reverse angle diff %0.4f\n",forwardAngDiff,reverseAngDiff);

	if (abs(forwardAngDiff) > abs(reverseAngDiff))
		maxDirection = true;
	else
		maxDirection = false;

	maxEdgeID(0, 2) = (int) maxDirection;

	if (maxDirection) {
		maxEdgeP(0, 0) = get2DReverseLogPx(maxEdgePos, position, icov);
	} else {
		maxEdgeP(0, 0) = get2DForwardLogPx(maxEdgePos, position, icov);
	}

	delete localGraph;
}

void outputLocalHypothesis(mxArray **plhs, const mxArray **prhs,
		CatGraph *catGraph) {

	const Eigen::Map<Eigen::MatrixXd> hypothesisDist(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> particleNo(mxGetPr(prhs[2]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> maxFrame(mxGetPr(prhs[3]), 1, 1);

	if (particleNo(0, 0) > catGraph->particles.size()) {
		mexErrMsgTxt("Particle index greater than numParticles");
	}

	Particle *particle = catGraph->particles.at((int) particleNo(0, 0));
	LocalGraph *localGraph = new LocalGraph(particle, 5 * hypothesisDist(0, 0)
			+ 1.0, (unsigned int) maxFrame(0, 0));

	//mexPrintf("Local graph with %d nodes, %d edges, %d particles\n",
	//		localGraph->nodes.size(), localGraph->edges.size(),
	//		localGraph->numParticles);

	map<Particle*, Vector3d> particles;
	Vector3d position;

	plhs[7] = mxCreateDoubleMatrix(1, 1, mxREAL);
	Eigen::Map<Eigen::MatrixXd> hypothesis(mxGetPr(plhs[7]), 1, 1);
	hypothesis(0, 0) = 0;

	for (set<GraphEdge*>::iterator edgeIter = localGraph->edges.begin(); edgeIter
			!= localGraph->edges.end(); edgeIter++) {
		for (vector<Particle*>::iterator edgeParticleIter =
				(*edgeIter)->particles.begin(); edgeParticleIter
				!= (*edgeIter)->particles.end(); edgeParticleIter++) {
			position = forwardModelFrac(localGraph->nodes.find(
					(*edgeIter)->nodeFrom)->second, (*edgeIter)->odometry,
					(*edgeParticleIter)->fraction);
			if ((*edgeParticleIter)->direction)
				position(2) = constrainAngle(position(2) + pi);
			double distance = getRadius(position);
			double forwardAngDiff = angleDistance(position(2), 0);
			double reverseAngDiff = angleDistance(constrainAngle(position(2)
					+ pi), 0);
			//mexPrintf(
			//		"Particle %X at relative position %0.4f %0.4f %0.4f distance %0.4f fAD %0.4f rAD %0.4f weight %0.4f\n",
			//		*edgeParticleIter, position(0), position(1), position(2),
			//		distance, forwardAngDiff, reverseAngDiff,
			//		(*edgeParticleIter)->weight);
			if (abs(reverseAngDiff) > abs(forwardAngDiff) && distance
					< hypothesisDist(0, 0)) {
				//mexPrintf("   --> Added to hypothesis\n");
				particles.insert(pair<Particle*, Vector3d> (*edgeParticleIter,
						position));
			}
		}
	}

	//mexPrintf("Total particles: %d Total weight: %0.4f\n", particles.size(),
	//		hypothesis(0, 0));
	unsigned int numParticles = particles.size();

	plhs[0] = mxCreateDoubleMatrix(2, numParticles, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[5] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[6] = mxCreateDoubleMatrix(3, numParticles, mxREAL);

	Eigen::Map<Eigen::MatrixXd> edgeID(mxGetPr(plhs[0]), 2, numParticles);
	Eigen::Map<Eigen::MatrixXd> fraction(mxGetPr(plhs[1]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> direction(mxGetPr(plhs[2]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd>
			odoLikelihood(mxGetPr(plhs[3]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd>
			visLikelihood(mxGetPr(plhs[4]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> weight(mxGetPr(plhs[5]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> positions(mxGetPr(plhs[6]), 3, numParticles);

	unsigned int i = 0;
	for (map<Particle*, Vector3d>::iterator mapIter = particles.begin(); mapIter
			!= particles.end(); mapIter++) {

		//mexPrintf("Adding %0.4f from Particle %X\n", mapIter->first->weight
		//		/ (1.0 + getRadius(mapIter->second)), mapIter->first);

		hypothesis(0, 0) += mapIter->first->weight / (1.0 + getRadius(
				mapIter->second));

		edgeID(0, i) = mapIter->first->root->nodeFrom->frameNum;
		edgeID(1, i) = mapIter->first->root->nodeTo->frameNum;
		fraction(0, i) = mapIter->first->fraction;
		direction(0, i) = (double) mapIter->first->direction;
		odoLikelihood(0, i) = mapIter->first->odoLikelihood;
		visLikelihood(0, i) = mapIter->first->visLikelihood;
		weight(0, i) = mapIter->first->weight;
		positions.block<3, 1> (0, i) = mapIter->second;
		i++;
	}

}

void outputNodes(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	unsigned int numNodes = catGraph->nodes.size();

	// Uncomment for FAB-MAP like comparisons
	catGraph->recomputeLikelihoods();

	plhs[0] = mxCreateDoubleMatrix(1, numNodes, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(1, numNodes, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, numNodes, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(1, numNodes, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(1, numNodes, mxREAL);
	plhs[5] = mxCreateDoubleMatrix(1, 1, mxREAL);
	plhs[6] = mxCreateDoubleMatrix(1, 1, mxREAL);

	Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(plhs[0]), 1, numNodes);
	Eigen::Map<Eigen::MatrixXd> edgesFrom(mxGetPr(plhs[1]), 1, numNodes);
	Eigen::Map<Eigen::MatrixXd> edgesTo(mxGetPr(plhs[2]), 1, numNodes);
	Eigen::Map<Eigen::MatrixXd> odoLikelihood(mxGetPr(plhs[3]), 1, numNodes);
	Eigen::Map<Eigen::MatrixXd> visLikelihood(mxGetPr(plhs[4]), 1, numNodes);
	Eigen::Map<Eigen::MatrixXd> odoLikelihood_mean(mxGetPr(plhs[5]), 1, 1);
	Eigen::Map<Eigen::MatrixXd> visLikelihood_mean(mxGetPr(plhs[6]), 1, 1);

	for (unsigned int i = 0; i < numNodes; i++) {
		frameNum(0, i) = catGraph->nodes[i]->frameNum;
		edgesFrom(0, i) = catGraph->nodes[i]->edgeFrom.size();
		edgesTo(0, i) = catGraph->nodes[i]->edgeTo.size();
		odoLikelihood(0, i) = catGraph->nodes[i]->odoLikelihood;
		visLikelihood(0, i) = catGraph->nodes[i]->visLikelihood;
	}

	odoLikelihood_mean(0, 0) = catGraph->odoLikelihood_mean;
	visLikelihood_mean(0, 0) = catGraph->visLikelihood_mean;

}

void outputEdges(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	unsigned int numEdges = catGraph->edges.size();

	plhs[0] = mxCreateDoubleMatrix(1, numEdges, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(3, numEdges, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, numEdges, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(1, numEdges, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(1, numEdges, mxREAL);

	Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(plhs[0]), 1, numEdges);
	Eigen::Map<Eigen::MatrixXd> odometry(mxGetPr(plhs[1]), 3, numEdges);
	Eigen::Map<Eigen::MatrixXd> nodeFrom(mxGetPr(plhs[2]), 1, numEdges);
	Eigen::Map<Eigen::MatrixXd> nodeTo(mxGetPr(plhs[3]), 1, numEdges);
	Eigen::Map<Eigen::MatrixXd> numParticles(mxGetPr(plhs[4]), 1, numEdges);

	for (unsigned int i = 0; i < numEdges; i++) {
		frameNum(0, i) = catGraph->edges[i]->frameNum;
		odometry.block<3, 1> (0, i) = catGraph->edges[i]->odometry;
		nodeFrom(0, i) = catGraph->edges[i]->nodeFrom->frameNum;
		nodeTo(0, i) = catGraph->edges[i]->nodeTo->frameNum;
		numParticles(0, i) = catGraph->edges[i]->particles.size();
	}

}

void outputParticles(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	unsigned int numParticles = catGraph->particles.size();

	plhs[0] = mxCreateDoubleMatrix(2, numParticles, mxREAL);
	plhs[1] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[2] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[3] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[4] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[5] = mxCreateDoubleMatrix(1, numParticles, mxREAL);
	plhs[6] = mxCreateDoubleMatrix(1, numParticles, mxREAL);

	Eigen::Map<Eigen::MatrixXd> edgeID(mxGetPr(plhs[0]), 2, numParticles);
	Eigen::Map<Eigen::MatrixXd> fraction(mxGetPr(plhs[1]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> direction(mxGetPr(plhs[2]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd>
			odoLikelihood(mxGetPr(plhs[3]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd>
			visLikelihood(mxGetPr(plhs[4]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> weight(mxGetPr(plhs[5]), 1, numParticles);
	Eigen::Map<Eigen::MatrixXd> hypothesis(mxGetPr(plhs[6]), 1, numParticles);

	for (unsigned int i = 0; i < numParticles; i++) {
		edgeID(0, i) = catGraph->particles[i]->root->nodeFrom->frameNum;
		edgeID(1, i) = catGraph->particles[i]->root->nodeTo->frameNum;
		fraction(0, i) = catGraph->particles[i]->fraction;
		direction(0, i) = (double) catGraph->particles[i]->direction;
		odoLikelihood(0, i) = catGraph->particles[i]->odoLikelihood;
		visLikelihood(0, i) = catGraph->particles[i]->visLikelihood;
		weight(0, i) = catGraph->particles[i]->weight;
		hypothesis(0, i) = catGraph->particles[i]->hypothesis;
	}

}

void outputPlan(mxArray **plhs, const mxArray **prhs, CatGraph *catGraph) {

	const Eigen::Map<Eigen::MatrixXd> startFrame(mxGetPr(prhs[1]), 1, 1);
	const Eigen::Map<Eigen::MatrixXd> goalFrame(mxGetPr(prhs[2]), 1, 1);

	GraphNode* startNode = NULL;
	GraphNode* goalNode = NULL;

	GraphNode* node;
	for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
			!= catGraph->nodes.end(); nodeIter++) {
		node = *nodeIter;
		if (node->frameNum == (unsigned int) startFrame(0, 0))
			startNode = *nodeIter;
		if (node->frameNum == (unsigned int) goalFrame(0, 0))
			goalNode = *nodeIter;
	}
	if (startNode == NULL || goalNode == NULL) {
		mexPrintf("Could not find corresponding node for start %X or goal %X\n",startNode,goalNode);
		mexErrMsgTxt("Quitting...");
	}

	map<GraphNode*,double> minDistance;
	map<GraphNode*, GraphNode*> previous;

	for (vector<GraphNode*>::iterator nodeIter = catGraph->nodes.begin(); nodeIter
			!= catGraph->nodes.end(); nodeIter++) {
		minDistance[*nodeIter] = DBL_MAX;
	}
	minDistance[startNode] = 0;

	set<pair<double, GraphNode*> > nodeQueue;
	nodeQueue.insert(pair<double, GraphNode*> (minDistance[startNode],
			startNode));

	GraphNode* currNode;
	while (!nodeQueue.empty()) {
		currNode = nodeQueue.begin()->second;
		if (currNode == goalNode)
			break;

		nodeQueue.erase(nodeQueue.begin());

		GraphEdge* currEdge;
		for (vector<GraphEdge*>::iterator edgeIter = currNode->edgeFrom.begin(); edgeIter
				!= currNode->edgeFrom.end(); edgeIter++) {
			currEdge = *edgeIter;
			double distanceSum = minDistance[currNode] + getRadius(currEdge->odometry);
			if (distanceSum < minDistance[currEdge->nodeFrom]) {
				nodeQueue.erase(pair<double,GraphNode*> (minDistance[currEdge->nodeFrom],currEdge->nodeFrom));
				minDistance[currEdge->nodeFrom] = distanceSum;
				previous[currEdge->nodeFrom] = currNode;
				nodeQueue.insert(pair<double,GraphNode*> (minDistance[currEdge->nodeFrom],currEdge->nodeFrom));
			}
		}
		for (vector<GraphEdge*>::iterator edgeIter = currNode->edgeTo.begin(); edgeIter
						!= currNode->edgeTo.end(); edgeIter++) {
			currEdge = *edgeIter;
			double distanceSum = minDistance[currNode] + getRadius(
					currEdge->odometry);
			if (distanceSum < minDistance[currEdge->nodeTo]) {
				nodeQueue.erase(pair<double, GraphNode*> (
						minDistance[currEdge->nodeTo], currEdge->nodeTo));
				minDistance[currEdge->nodeTo] = distanceSum;
				previous[currEdge->nodeTo] = currNode;
				nodeQueue.insert(pair<double, GraphNode*> (
						minDistance[currEdge->nodeTo], currEdge->nodeTo));
			}
		}

	}

	list<GraphNode*> path;
	map<GraphNode*,GraphNode*>::iterator prev;
	currNode = goalNode;
	path.push_front(currNode);
	while ((prev = previous.find(currNode)) != previous.end()) {
		currNode = prev->second;
		path.push_front(currNode);
	}

	unsigned int numNodes = path.size();

	plhs[0] = mxCreateDoubleMatrix(1, numNodes, mxREAL);

	Eigen::Map<Eigen::MatrixXd> frameNum(mxGetPr(plhs[0]), 1, numNodes);

	unsigned int i = 0;
	for (list<GraphNode*>::iterator pathIter = path.begin(); pathIter != path.end(); pathIter++) {
		frameNum(0, i) = (*pathIter)->frameNum;
		i++;
	}
}

